// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectAlignment.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Cameras.Kinect
{
    using System;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Kinect;
    using Microsoft.Robotics.Numerics;
    
    /// <summary>
    /// Creates the mapping between the depth and color images
    /// </summary>
    public class KinectAlignment : ColorDepthAlignment
    {
        /// <summary>
        /// The class that maps between the depth and the color image
        /// </summary>
        private CoordinateMapper mapper;

        /// <summary>
        /// Maps the depth to the color
        /// </summary>
        private ColorImagePoint[] depthToColor;

        /// <summary>
        /// Maps the color to the depth
        /// </summary>
        private DepthImagePoint[] colorToDepth;

        /// <summary>
        /// Maps the color to 3D
        /// </summary>
        private SkeletonPoint[] colorToSkeleton;

        /// <summary>
        /// Maps the depth to 3D
        /// </summary>
        private SkeletonPoint[] depthToSkeleton;

        /// <summary>
        /// The size of the depth image
        /// </summary>
        private Size2D<int> depthSize;

        /// <summary>
        /// The size of the color image
        /// </summary>
        private Size2D<int> colorSize;

        /// <summary>
        /// The passed in depth image
        /// </summary>
        private ImageFrame<short> depthImage;

        /// <summary>
        /// The passed in color image
        /// </summary>
        private ImageFrame<RgbPix> rgbImage;

        /// <summary>
        /// The depth pixels
        /// </summary>
        private DepthImagePixel[] depthPixels;

        /// <summary>
        /// A flag indicating if the images are left right
        /// </summary>
        private bool leftRightImages;

        /// <summary>
        /// The alignment parameters
        /// </summary>
        private ReadOnlyCollection<byte> alignmentParams;

        /// <summary>
        /// Initializes a new instance of the KinectAlignment class
        /// </summary>
        /// <param name="depthParams">The depth camera parameters</param>
        /// <param name="colorParams">The color camera parameters</param>
        public KinectAlignment(CameraParameters depthParams, CameraParameters colorParams)
        {
            KinectDepthCameraParameters kinectDepthParams = (KinectDepthCameraParameters)depthParams;

            this.alignmentParams = new ReadOnlyCollection<byte>((byte[])kinectDepthParams.GetAlignmentParameters());
            this.mapper = new CoordinateMapper(this.alignmentParams);

            this.leftRightImages = kinectDepthParams.LeftRightImage;
            this.depthToColor = null;
            this.colorToDepth = null;
            this.depthToSkeleton = null;
            this.colorToSkeleton = null;
        }

        /// <summary>
        /// Computes the mapping given a depth image
        /// </summary>
        /// <param name="depth">The depth image</param>
        /// <param name="color">the color image</param>
        /// <param name="depthParams">the depth parameters</param>
        /// <param name="colorParams">The color parameters</param>
        public override void Align(ImageFrame<short> depth, ImageFrame<RgbPix> color, CameraParameters depthParams, CameraParameters colorParams)
        {
            this.depthSize = new Size2D<int>(depth.Width, depth.Height);
            this.colorSize = new Size2D<int>(color.Width, color.Height);

            this.depthImage = depth;
            this.rgbImage = color;

            // reallocate memory if the image sizes have changed
            if (this.depthToColor == null || this.depthSize.Width * this.depthSize.Height != this.depthToColor.Length)
            {
                int size = this.depthSize.Width * this.depthSize.Height;
                this.depthToColor = new ColorImagePoint[size];
                this.depthToSkeleton = new SkeletonPoint[size];
                this.depthPixels = new DepthImagePixel[size];
            }

            if (this.colorToDepth == null || this.colorSize.Width * this.colorSize.Height != this.colorToDepth.Length)
            {
                int size = this.colorSize.Width * this.colorSize.Height;
                this.colorToDepth = new DepthImagePoint[size];
                this.colorToSkeleton = new SkeletonPoint[size];
            }

            // Convert ImageFrameDepth to DepthPixel[]
            Parallel.For(
                0,
                depth.Height,
                row =>
                {
                    int offset = row * depth.Width;
                    for (int col = 0; col < depth.Width; col++)
                    {
                        if (this.leftRightImages == true)
                        {
                            // reverse the depth image because the coordinate mapper requires a mirrored depth image
                            depthPixels[offset + col].Depth = depth[offset + (depth.Width - col - 1)];
                        }
                        else
                        {
                            depthPixels[offset + col].Depth = depth[offset + col];
                        }
                    }
                });

            DepthImageFormat depthFormat = this.Format(depth);
            ColorImageFormat colorFormat = this.Format(color);

            // Compute mappings
            this.mapper.MapDepthFrameToColorFrame(depthFormat, this.depthPixels, colorFormat, this.depthToColor);
            this.mapper.MapColorFrameToDepthFrame(colorFormat, depthFormat, this.depthPixels, this.colorToDepth);
            this.mapper.MapColorFrameToSkeletonFrame(colorFormat, depthFormat, this.depthPixels, this.colorToSkeleton);
            this.mapper.MapDepthFrameToSkeletonFrame(depthFormat, this.depthPixels, this.depthToSkeleton);
        }
        
        /// <summary>
        /// Inline function that returns a depth pixel mapped to a color pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The color of the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override RgbPix ColorOfDepthPixel(int x, int y)
        {
            ColorImagePoint point;

            if (this.leftRightImages)
            {
                point = this.depthToColor[y * this.depthImage.Width + (this.depthImage.Width - x - 1)];
            }
            else
            {
                point = this.depthToColor[y * this.depthImage.Width + x];
            }

            if (point.X >= 0 && point.X < this.rgbImage.Width && point.Y >= 0 && point.Y < this.rgbImage.Height)
            {
                if (this.leftRightImages)
                {
                    return this.rgbImage[point.Y * this.rgbImage.Width + (this.rgbImage.Width - point.X - 1)];
                }
                else
                {
                    return this.rgbImage[point.Y * this.rgbImage.Width + point.X];
                }
            }

            return new RgbPix(0, 0, 0);
        }

        /// <summary>
        /// Inline function that returns the location of the depth pixel in the color image
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The location in the color image</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override Point2D<int>? LocationOfDepthPixelinColor(int x, int y)
        {
            ColorImagePoint point;

            if (this.leftRightImages)
            {
                point = this.depthToColor[y * this.depthImage.Width + (this.depthImage.Width - x - 1)];
                return new Point2D<int>(this.rgbImage.Width - point.X - 1, point.Y);
            }
            else
            {
                point = this.depthToColor[y * this.depthImage.Width + x];
                return new Point2D<int>(point.X, point.Y);
            }
        }

        /// <summary>
        /// Inline function that returns a color pixel mapped to a depth pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The depth at the color pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override short DepthOfRgbPixel(int x, int y)
        {
            DepthImagePoint point;
            if (this.leftRightImages)
            {
                point = this.colorToDepth[y * this.colorSize.Width + (this.colorSize.Width - x - 1)];
            }
            else
            {
                point = this.colorToDepth[y * this.colorSize.Width + x];
            }

            if (point.X >= 0 && point.X < this.depthImage.Width && point.Y >= 0 && point.Y < this.depthImage.Height)
            {
                if (this.leftRightImages)
                {
                    return this.depthImage[point.Y * this.depthImage.Width + (this.depthImage.Width - point.X - 1)];
                }
                else
                {
                    return this.depthImage[point.Y * this.depthImage.Width + point.X];
                }
            }

            return 0;
        }

        /// <summary>
        /// Inline function that returns the location of the color pixel in the depth image
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The location in the depth image</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override Point2D<int>? LocationOfColorPixelinDepth(int x, int y)
        {
            DepthImagePoint point;
            if (this.leftRightImages)
            {
                point = this.colorToDepth[y * this.colorSize.Width + (this.colorSize.Width - x - 1)];
                return new Point2D<int>(this.depthSize.Width - point.X - 1, point.Y);
            }
            else
            {
                point = this.colorToDepth[y * this.colorSize.Width + x];
                return new Point2D<int>(point.X, point.Y);
            }
        }

        /// <summary>
        /// Inline function that returns the 3D point of a color pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">The y value of the pixel</param>
        /// <returns>The 3D point projected onto the color image</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override Vector3? ColorPixelTo3D(int x, int y)
        {
            SkeletonPoint point;
            if (this.leftRightImages)
            {
                point = this.colorToSkeleton[y * this.colorSize.Width + (this.colorSize.Width - x - 1)];
            }
            else
            {
                point = this.colorToSkeleton[y * this.colorSize.Width + x];
            }

            if (KinectSensor.IsKnownPoint(point))
            {
                return new Vector3(-point.X, -point.Y, point.Z);
            }

            return null;
        }

        /// <summary>
        /// Inline function that returns the 3D point of a depth pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">The y value of the pixel</param>
        /// <returns>The 3D point projected onto the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override Vector3? DepthPixelTo3D(int x, int y)
        {
            SkeletonPoint point;

            if (this.leftRightImages)
            {
                point = this.depthToSkeleton[y * this.depthImage.Width + (this.depthImage.Width - x - 1)];
            }
            else
            {
                point = this.depthToSkeleton[y * this.depthImage.Width + x];
            }

            return new Vector3(-point.X, -point.Y, point.Z);
        }

        /// <summary>
        /// Inline function that returns the projection of a 3D point onto the color image
        /// </summary>
        /// <param name="point">The 3D point</param>
        /// <returns>The 3D point projected onto the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override Vector2? Point3DToColor(Vector3 point)
        {
            SkeletonPoint skeleton = new SkeletonPoint();
            skeleton.X = -(float)point.X;
            skeleton.Y = -(float)point.Y;
            skeleton.Z = (float)point.Z;

            ColorImageFormat format;
            if (this.rgbImage.ImageData.Length == 640 * 480)
            {
                format = ColorImageFormat.RgbResolution640x480Fps30;
            }
            else
            {
                format = ColorImageFormat.RgbResolution1280x960Fps12;
            }

            ColorImagePoint projection = this.mapper.MapSkeletonPointToColorPoint(skeleton, format);

            if (this.leftRightImages)
            {
                return new Vector2(this.rgbImage.Width - projection.X - 1, projection.Y);
            }
            else
            {
                return new Vector2(projection.X, projection.Y);
            }
        }

        /// <summary>
        /// Inline function that returns the projection of a 3D point onto the depth image
        /// </summary>
        /// <param name="point">The 3D point</param>
        /// <returns>The 3D point projected onto the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public override Vector2? Point3DToDepth(Vector3 point)
        {
            SkeletonPoint skeleton = new SkeletonPoint();
            skeleton.X = -(float)point.X;
            skeleton.Y = -(float)point.Y;
            skeleton.Z = (float)point.Z;

            DepthImageFormat format;
            if (this.depthImage.ImageData.Length == 80 * 60)
            {
                format = DepthImageFormat.Resolution80x60Fps30;
            }
            else if (this.depthImage.ImageData.Length == 320 * 240)
            {
                format = DepthImageFormat.Resolution320x240Fps30;
            }
            else
            {
                format = DepthImageFormat.Resolution640x480Fps30;
            }

            DepthImagePoint projection = this.mapper.MapSkeletonPointToDepthPoint(skeleton, format);

            if (this.leftRightImages)
            {
                return new Vector2(this.depthImage.Width - projection.X - 1, projection.Y);
            }
            else
            {
                return new Vector2(projection.X, projection.Y);
            }
        }

        /// <summary>
        /// Returns the format of the depth image
        /// </summary>
        /// <param name="image">the depth image</param>
        /// <returns>The format of the depth image</returns>
        private DepthImageFormat Format(ImageFrame<short> image)
        {
            if (image.Width == 640 && image.Height == 480)
            {
                return DepthImageFormat.Resolution640x480Fps30;
            }
            else if (image.Width == 320 && image.Height == 240)
            {
                return DepthImageFormat.Resolution320x240Fps30;
            }
            else if (image.Width == 80 && image.Height == 60)
            {
                return DepthImageFormat.Resolution80x60Fps30;
            }

            return DepthImageFormat.Undefined;
        }

        /// <summary>
        /// Returns the format of the color image
        /// </summary>
        /// <param name="image">the color image</param>
        /// <returns>the format of the color image</returns>
        private ColorImageFormat Format(ImageFrame<RgbPix> image)
        {
            if (image.Width == 640 && image.Height == 480)
            {
                return ColorImageFormat.RgbResolution640x480Fps30;
            }
            else if (image.Width == 1280 && image.Height == 960)
            {
                return ColorImageFormat.RgbResolution1280x960Fps12;
            }

            return ColorImageFormat.Undefined;
        }
    }
}
